#include "stabilization.h"
#include "a2d.h"
#include <avr/eeprom.h>
#include <math.h>
#include "misc.h"


// IR-sensor 

unsigned int   EEMEM IRmaxEeprom;
unsigned int   EEMEM RlevelEeprom;
unsigned int   EEMEM PlevelEeprom;


unsigned int   EEMEM rpEeprom;
unsigned int   EEMEM riEeprom;
unsigned int   EEMEM rdEeprom;

unsigned int   EEMEM ppEeprom;
unsigned int   EEMEM piEeprom;
unsigned int   EEMEM pdEeprom;

unsigned int   EEMEM spEeprom;
unsigned int   EEMEM siEeprom;
unsigned int   EEMEM sdEeprom;



static char  rollport,pitchport;
static int   IRmax =800;		// Max value from IR-Sensor
static int   RAmax =60;		// Max Roll angle from GetRollangle
static int   PAmax =60;		// Max Pitch angle from GetPollangle
static int   Rservomax =400;		// Max roll servo signal
static int   Pservomax =400;		// Max pitch servo signal


int   Rlevel=512;
int   Plevel=512;

int   Ractual,Pactual;  	// Raw values from infrared sensors
static double  Rerr, Perr;
static double  Rerr_i,Perr_i;
static double  Rerr_d,Perr_d;
static double  rp,ri,rd;
static double  pp,pi,pd;
static double  sp,si,sd;

 double Rout=0;
 double Pout=0;
 double Sout=0;




int      Rdiff;


int   Pdiff;


int calibrate() {
	static char j=0;
	static int max=0;
//	for (j=0;j<10;j++) {
//		max=max+a2dConvert10bit(2);   
//	}
//	IRmax=max/10;

	IRmax=650;
	IRmax=1023-a2dConvert10bit(2);
	eeprom_write_word((void*)&IRmaxEeprom,(int)IRmax);
	return IRmax;
}

int getIRmax() {
	return IRmax;
}



void setPid(int pidId, double pid_value) {
	switch(pidId) {
		case 1:  rp=pid_value/100;
 			 	 eeprom_write_word((void*)&rpEeprom,(int)pid_value);
		 		 break;
		case 2:  ri=pid_value/100;
		 		 eeprom_write_word((void*)&riEeprom,(int)pid_value);
		 		 break;
		case 3:  rd=pid_value/100;
		 		 eeprom_write_word((void*)&rdEeprom,(int)pid_value);
		 		 break;
		case 4:  pp=pid_value/100;
		 		 eeprom_write_word((void*)&ppEeprom,(int)pid_value);
		 		 break;
		case 5:  pi=pid_value/100;
		 		 eeprom_write_word((void*)&piEeprom,(int)pid_value);
		 		 break;
		case 6:  pd=pid_value/100;
		 		 eeprom_write_word((void*)&pdEeprom,(int)pid_value);
		 		 break;
		case 7:  sp=pid_value/100;
		 		 eeprom_write_word((void*)&spEeprom,(int)pid_value);
		 		 break;
		case 8:  si=pid_value/100;
		 		 eeprom_write_word((void*)&siEeprom,(int)pid_value);
		 		 break;
		case 9:  sd=pid_value/100;
		 		 eeprom_write_word((void*)&sdEeprom,(int)pid_value);
		 		 break;
	} // switch
}



void setIR(int irId, int ir_value) {
	switch(irId) {
		case 1:  IRmax=ir_value;
 			 	 eeprom_write_word((void*)&IRmaxEeprom,(int)ir_value);
		 		 break;
		case 2:  Rlevel=ir_value;
 			 	 eeprom_write_word((void*)&RlevelEeprom,(int)ir_value);
		 		 break;
		case 3:  Plevel=ir_value;
 			 	 eeprom_write_word((void*)&PlevelEeprom,(int)ir_value);
		 		 break;
	} // switch
}


int getPid(int pidId){
	static double temp=0; 
 	switch(pidId) {
		case 1:  temp=(rp*100);
		 		 break;
		case 2:  temp=(ri*100);
		 		 break;
		case 3:  temp=(rd*100);
		 		 break;
		case 4:  temp=(pp*100);
		 		 break;
		case 5:  temp=(pi*100);
		 		 break;
		case 6:  temp=(pd*100);
		 		 break;
		case 7:  temp=(sp*100);
		 		 break;
		case 8:  temp=(si*100);
		 		 break;
		case 9:  temp=(sd*100);
		 		 break;
	} // switch
	return (int)temp;
}

int getIR(int pidId){

 	switch(pidId) {
		case 1:  return IRmax;
		 		 break;
		case 2:  return Rlevel;
		 		 break;
		case 3:  return Plevel;
		 		 break;
	} // switch

}

void initStabilization(char rollp, char pitchp) {
	static double temp=0;
	// A/D
	a2dInit();
	a2dSetReference(ADC_REFERENCE_AVCC);
	rollport=rollp;
	pitchport=pitchp;

	//read Ir settings from Eeprom
	IRmax=eeprom_read_word(&IRmaxEeprom);
	Rlevel=eeprom_read_word(&RlevelEeprom);
	Plevel=eeprom_read_word(&PlevelEeprom);
		
	//read PID values from Eprom
	temp=eeprom_read_word(&rpEeprom);
	rp=temp/100;
	temp=eeprom_read_word(&riEeprom);
	ri=temp/100;
	temp=eeprom_read_word(&rdEeprom);
	rd=temp/100;

	temp=eeprom_read_word(&ppEeprom);
	pp=temp/100;
	temp=eeprom_read_word(&piEeprom);
	pi=temp/100;
	temp=eeprom_read_word(&pdEeprom);
	pd=temp/100;
	
	temp=eeprom_read_word(&spEeprom);
	sp=temp/100;
	temp=eeprom_read_word(&siEeprom);
	si=temp/100;
	temp=eeprom_read_word(&sdEeprom);
	sd=temp/100;
}


double get_roll_angle() {
	static double ra=0;

//  sensor mounted 90 deg
//	Ractual=Ractual*0.5+a2dConvert10bit(1)*0.5;   

//	sensor mounted 45 deg   
	Ractual=Ractual/2+(a2dConvert10bit(0)+(1023-a2dConvert10bit(1)))/4;
	ra=(Ractual-Rlevel);
	ra=ra/(IRmax-Rlevel);
	ra=ra*-45;
	if (ra>RAmax)
		ra=RAmax;
	if (ra<-RAmax)
		ra=-RAmax;
	return ra;
}

int get_roll_raw() {
	return a2dConvert10bit(1);   
}

int get_pitch_raw() {
	return a2dConvert10bit(0);   
}

double get_pitch_angle() {
	static double pa=0;

//  sensor mounted 90 deg
//	Pactual=Pactual*0.5+a2dConvert10bit(0)*0.5;   

//	sensor mounted 45 deg   
	Pactual=Pactual/2+(a2dConvert10bit(0)+a2dConvert10bit(1))/4;

	pa=(Pactual-Plevel);
	pa=pa/(IRmax-Plevel);
	pa=pa*-45;
	if (pa>PAmax)
		pa=PAmax;
	if (pa<-PAmax)
		pa=-PAmax;
	return pa;
}

int do_roll_PID(int rtarget, double Rangle) {
	Rerr_d=Rerr;
	Rerr= Rangle-rtarget;
	Rerr_d=Rerr-Rerr_d;
	Rerr_i=Rerr_i+Rerr;
	if (Rerr_i>1000)
		Rerr_i=1000;	
	if (Rerr_i<-1000)
		Rerr_i=-1000;	
	Rout=Rerr*rp;
	Rout=Rout+Rerr_i*ri;
	Rout=Rout+Rerr_d*rd;
	if (Rout>Rservomax)
		Rout=Rservomax;
	if (Rout<-Rservomax)
		Rout=-Rservomax;
	return (int)(Rout);
}

int do_pitch_PID(int ptarget, double Pangle) {
	Perr_d=Perr;
	Perr = Pangle-ptarget;
	Perr_d=Perr-Perr_d;
	Perr_i=Perr_i+Perr;
	if (Perr_i>1000)
		Perr_i=1000;	
	if (Perr_i<-1000)
		Perr_i=-1000;	
	Pout=Perr*pp;
	Pout=Pout+Perr_i*pi;
	Pout=Pout+Perr_d*pd;
	if (Pout>Pservomax)
		Pout=Pservomax;
	if (Pout<-Pservomax)
		Pout=-Pservomax;
	return (int)(Pout);
}
